#lang racket


(require "coord.rkt"
         "math.rkt"
         "range.rkt")


(provide m-rotate
         m.scale
         m.translate
         
         m.line
         m.column
         
         -m
         m+m
         m-m
         m*m
         m*p
         
         m.zero
         m.identity
         
         matrix<-lines
         matrix<-cols
         list<-matrix
         vector<-matrix)


(define (v*v v1 v2)
  (foldl + 0 (vector->list (vector-map * v1 v2))))


(define (matrix-print m port mode)  
  (define (print-line v)
    (write-string
     (string-join
      (vector->list
       (vector-map (λ (e) (format "~a" e)) v))
      " ")
     port))
  
  (write-string "m(" port)
  (print-line (vector-copy (matrix-vals m) 0 4))
  (newline port)
  
  (write-string "  " port)
  (print-line (vector-copy (matrix-vals m) 4 8))
  (newline port)
  
  (write-string "  " port)
  (print-line (vector-copy (matrix-vals m) 8 12))
  (newline port)
  
  (write-string "  " port)
  (print-line (vector-copy (matrix-vals m) 12 16))
  (write-string ")" port))

(define-struct matrix
  (vals)
  #:property prop:custom-write matrix-print)

(define (m-rotate a n)
  (define (m.rotate-aux n1 n2 n3)
    (let ((c (cos a))
          (s (sin a))
          (t (- 1 (cos a)))
          (p (norm (xyz n1 n2 n3))))
      (let ((x (xyz-x p))
            (y (xyz-y p))
            (z (xyz-z p)))
        (let ((r00 (+ (* t (^2 x)) c))
              (r01 (- (* t x y) (* s z)))
              (r02 (+ (* t x z) (* s y)))
              
              (r10 (+ (* t x y) (* s z)))
              (r11 (+ (* t (^2 y)) c))
              (r12 (- (* t y z) (* s x)))
              
              (r20 (- (* t x y) (* s y)))
              (r21 (+ (* t y z) (* s x)))
              (r22 (+ (* t (^2 z)) c)))
          (make-matrix
           (vector r00 r01 r02 0
                   r10 r11 r12 0
                   r20 r21 r22 0
                   0   0   0   1))))))
  (if (or (= a 0) (eq-c n u0))
      m.identity
      (apply m.rotate-aux (list-of-coord n))))

(define (m.scale x y z)
  (make-matrix
   (vector x 0 0 0
           0 y 0 0
           0 0 z 0
           0 0 0 1)))

(define (m.translate x y z)
  (make-matrix
   (vector 1 0 0 x
           0 1 0 y
           0 0 1 z
           0 0 0 1)))

(define (m.line m l)
  (vector-copy (matrix-vals m) (* l 4) (+ (* l 4) 4)))

(define (m.column m c)
  (let ((vals (matrix-vals m)))
    (vector
     (vector-ref vals c)
     (vector-ref vals (+ c 4))
     (vector-ref vals (+ c 8))
     (vector-ref vals (+ c 12)))))


(define (-m m)
  (make-matrix
   (vector-append
    (vector-map - (vector-copy (matrix-vals m) 0 12))
    (vector 0 0 0 1))))

(define (m+m m1 m2)
  (let ((vals1 (vector-take (matrix-vals m1) 12))
        (vals2 (vector-take (matrix-vals m2) 12)))
    (make-matrix
     (vector-append
      (vector-map + vals1 vals2)
      (vector 0 0 0 1)))))

(define (m-m m1 m2)
  (m+m m1 (-m m2)))

(define (m*m m1 m2)
  (make-matrix
   (list->vector
    (flatten
     (for/list ((i (: 4)))
       (for/list ((j (: 4)))
         (v*v (m.line m1 i) (m.column m2 j))))))))

; edit: should not give the user the option of distinguishing between coords and vectors
(define (m*p m p (vector? #f))
  (let* ((w (if vector? 0 1))
         (v (vector (xyz-x p) (xyz-y p) (xyz-z p) w)))
    (xyz
     (v*v (m.line m 0) v)
     (v*v (m.line m 1) v)
     (v*v (m.line m 2) v))))


(define m.zero
  (make-matrix
   (vector 0 0 0 0
           0 0 0 0
           0 0 0 0
           0 0 0 0)))

(define m.identity
  (m.scale 1 1 1))


(define (matrix<-lines line1 line2 line3)
  (make-matrix (list->vector (append line1 line2 line3 (list 0 0 0 1)))))

(define (matrix<-cols col1 col2 col3 col4)
  (apply matrix<-lines (map list col1 col2 col3 col4)))

(define (list<-matrix m)
  (vector->list (matrix-vals m)))

(define (vector<-matrix m)
  (vector-copy (matrix-vals m)))